﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Input;
using System.Diagnostics;

namespace datorgrafik_2
{
    class Tank : GameObject
    {
        private float elevation;

        private float robotLongitude = 0.0f;
        private float robotLatitude = 0.0f;

        private bool rotate = false;
        private float tireRotation = 0.0f;
        private float tankRotation = 0.0f;

        private Model tankModel;

        private HSL[,] heightData;

        private float robotWalkingDirection = 0.0f;
        ModelBone leftBackTireBone;
        ModelBone rightBackTireBone;
        ModelBone leftFrontTireBone;
        ModelBone rightFrontTireBone;
        ModelBone tankGeoBone;


        Matrix leftBackTireTransform;
        Matrix rightBackTireTransform;
        Matrix leftFrontTireTransform;
        Matrix rightFrontTireTransform;
        Matrix tankGeoTransform;

        Matrix[] boneTransforms;

        private float leftBackTireRotationValue;
        private float rightBackTireRotationValue;
        private float leftFrontTireRotationValue;
        private float rightFrontTireRotationValue;
        private float tankGeoRotationValue;

        private float leftBackTireElevation;
        private float rightBackTireElevation;
        private float leftFrontTireElevation;
        private float rightFrontTireElevation;


        private float heightPositionModifier = -0.15f;
        private GameTime gameTime;
        private float xrotate;
        private float zrotate;

        public float LeftBackTireRotation
        {
            get { return leftBackTireRotationValue; }
            set { leftBackTireRotationValue = value; }
        }

        public float RightBackTireRotation
        {
            get { return rightBackTireRotationValue; }
            set { rightBackTireRotationValue = value; }
        }

        public float LeftFrontTireRotation
        {
            get { return leftFrontTireRotationValue; }
            set { leftFrontTireRotationValue = value; }
        }

        public float RightFrontTireRotation
        {
            get { return rightFrontTireRotationValue; }
            set { rightFrontTireRotationValue = value; }
        }

        public float TankGeoRotation
        {
            get { return tankGeoRotationValue; }
            set { tankGeoRotationValue = value; }
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Tank" /> class.
        /// </summary>
        /// <param name="graphicsDevice">The graphics device.</param>
        /// <param name="Content">The content.</param>
        /// <param name="heightData">The height data in HSL form.</param>
        public Tank(GraphicsDevice graphicsDevice, ContentManager Content, HSL[,] heightData)
            : base(graphicsDevice)
        {
            tankModel = Content.Load<Model>("tank");
            position = new Vector3(0.0f, 0.0f, 0.0f);
            this.heightData = heightData;
            tankGeoBone = tankModel.Bones["tank_geo"];
            leftBackTireBone = tankModel.Bones["l_back_wheel_geo"];
            rightBackTireBone = tankModel.Bones["r_back_wheel_geo"];
            leftFrontTireBone = tankModel.Bones["l_front_wheel_geo"];
            rightFrontTireBone = tankModel.Bones["r_front_wheel_geo"];

            leftBackTireTransform = leftBackTireBone.Transform;
            rightBackTireTransform = rightBackTireBone.Transform;
            leftFrontTireTransform = leftFrontTireBone.Transform;
            rightFrontTireTransform = rightFrontTireBone.Transform;
            tankGeoTransform = tankGeoBone.Transform;

            boneTransforms = new Matrix[tankModel.Bones.Count];
        }

        /// <summary>
        /// Draws the gameobject.
        /// </summary>
        /// <param name="world">The world.</param>
        /// <param name="view">The view.</param>
        /// <param name="projection">The projection.</param>
        /// <param name="effect">The effect.</param>
        public void Draw(ref Matrix world, ref Matrix view, ref Matrix projection, ref BasicEffect effect)
        {
            DrawModel(tankModel, projection, view, world);
        }

        /// <summary>
        /// Updates the game object.
        /// </summary>
        /// <param name="gameTime">The game time.</param>
        public override void Update(GameTime gameTime)
        {
            
            this.gameTime = gameTime;
            
            CheckInput(gameTime);

            if (rotate)
            {
                LeftBackTireRotation = LeftBackTireRotation + tireRotation;
                RightBackTireRotation = RightBackTireRotation + tireRotation;
                LeftFrontTireRotation = LeftFrontTireRotation + tireRotation;
                RightFrontTireRotation = RightFrontTireRotation + tireRotation;
                TankGeoRotation = tankRotation;
                tankRotation = 0;
                rotate = false;
            }
            else
            {
                LeftBackTireRotation = 0;
                RightBackTireRotation = 0;
                LeftFrontTireRotation = 0;
                RightFrontTireRotation = 0;
            }

            // TODO: buggar här när man når polerna
            if (robotLatitude > 90) robotLatitude = 0f;
            if (robotLatitude < -90) robotLatitude = 0f;

            if (robotLongitude > 179) robotLongitude = -1f;
            if (robotLongitude < -179) robotLongitude = 1f;



            float delta = 0.01f * (float)gameTime.ElapsedGameTime.TotalMilliseconds;

            float floater = 1f;

            leftFrontTireElevation = LocalElevation(robotLatitude - floater, robotLongitude + floater, leftFrontTireElevation, delta);
            rightFrontTireElevation = LocalElevation(robotLatitude + floater, robotLongitude + floater, rightFrontTireElevation, delta);
            leftBackTireElevation = LocalElevation(robotLatitude - floater, robotLongitude - floater, leftBackTireElevation, delta);
            rightBackTireElevation = LocalElevation(robotLatitude + floater, robotLongitude - floater, rightBackTireElevation, delta);

            xrotate = (leftBackTireElevation + rightBackTireElevation) -
                             (leftFrontTireElevation + rightFrontTireElevation);

            zrotate = (rightBackTireElevation + rightFrontTireElevation) -
                                  (leftBackTireElevation + leftFrontTireElevation);



            elevation = LocalElevation(robotLatitude, robotLongitude, elevation, delta);


            base.Update(gameTime);

        }

        /// <summary>
        /// Checks the input.
        /// </summary>
        /// <param name="gameTime">The game time.</param>
        private void CheckInput(GameTime gameTime)
        {
            float movement = (float)gameTime.ElapsedGameTime.TotalMilliseconds * 0.005f;
            if (Keyboard.GetState().IsKeyDown(Keys.A))
            {
                robotLatitude += movement;
                tireRotation = 0.15f;
                tankRotation = MathHelper.PiOver2;
                rotate = true;
            }
            if (Keyboard.GetState().IsKeyDown(Keys.W))
            {
                robotLongitude += movement;
                tireRotation = 0.15f;
                rotate = true;
            }
            if (Keyboard.GetState().IsKeyDown(Keys.D))
            {
                robotLatitude -= movement;
                tireRotation = 0.15f;
                tankRotation = tankRotation - MathHelper.PiOver2;
                rotate = true;
            }
            if (Keyboard.GetState().IsKeyDown(Keys.S))
            {
                robotLongitude -= movement;
                tireRotation = 0.15f;
                tankRotation = tankRotation + MathHelper.Pi;
                rotate = true;
            }


            if (Keyboard.GetState().GetPressedKeys().Length == 2)
            {
                tankRotation = tankRotation / 2;
            }
            if (Keyboard.GetState().IsKeyDown(Keys.S) && Keyboard.GetState().IsKeyDown(Keys.D))
            {
                tireRotation = 0.15f;
                tankRotation = -(MathHelper.Pi / 2) - (MathHelper.PiOver4);
                rotate = true;
            }

          

            if (Keyboard.GetState().IsKeyDown(Keys.P))
            {
                heightPositionModifier += 0.01f;
            } if (Keyboard.GetState().IsKeyDown(Keys.O))
            {
                heightPositionModifier -= 0.01f;
            }

            
        }

        /// <summary>
        /// Draws the model.
        /// </summary>
        /// <param name="m">The model.</param>
        /// <param name="projection2">The projection.</param>
        /// <param name="view2">The view.</param>
        /// <param name="world">The world.</param>
        private void DrawModel(Model m, Matrix projection2, Matrix view2, Matrix world)
        {

            Matrix tankXrotation = Matrix.CreateRotationX(xrotate);
            Matrix tankZrotation = Matrix.CreateRotationZ(zrotate);


            Matrix tankRotation = Matrix.CreateRotationY(tankGeoRotationValue);
            Matrix leftBackTireRotation = Matrix.CreateRotationX(leftBackTireRotationValue);
            Matrix rightBackTireRotation = Matrix.CreateRotationX(rightBackTireRotationValue);
            Matrix leftFrontTireRotation = Matrix.CreateRotationX(leftFrontTireRotationValue);
            Matrix rightFrontTireRotation = Matrix.CreateRotationX(rightFrontTireRotationValue);
            m.Root.Transform = world * tankRotation * tankXrotation * tankZrotation;
            
            
            leftBackTireBone.Transform = leftBackTireRotation * leftBackTireTransform;
            rightBackTireBone.Transform = rightBackTireRotation * rightBackTireTransform;
            leftFrontTireBone.Transform = leftFrontTireRotation * leftFrontTireTransform;
            rightFrontTireBone.Transform = rightFrontTireRotation * rightFrontTireTransform;
            
            m.CopyAbsoluteBoneTransformsTo(boneTransforms);
            Matrix projection = projection2;
            Matrix view = view2;

            foreach (ModelMesh mesh in m.Meshes)
            {
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.EnableDefaultLighting();
                    effect.View = view;
                    effect.Projection = projection;
                    Matrix robotRotation;
                    float yaw = 0;
                    float pitch = 0;
                    float roll = 0;
                    GetYawPitchRollModel(out yaw, out pitch, out roll, robotLatitude, robotLongitude);
                    Quaternion.CreateFromYawPitchRoll(yaw, pitch, roll, out rotation);
                    Matrix.CreateFromQuaternion(ref rotation, out robotRotation);
                    effect.World = boneTransforms[mesh.ParentBone.Index] * Matrix.CreateScale(0.001f) *
                        robotRotation *
                        Matrix.CreateTranslation(EarthPosition(robotLatitude, robotLongitude, 50.0f));
                }
                mesh.Draw();
            }
        }


        /// <summary>
        /// Gets the yaw pitch and roll for the tank.
        /// </summary>
        /// <param name="yaw">The yaw.</param>
        /// <param name="pitch">The pitch.</param>
        /// <param name="roll">The roll.</param>
        /// <param name="robotLatitude">The tank latitude.</param>
        /// <param name="robotLongitude">The tank longitude.</param>
        private void GetYawPitchRollModel(out float yaw, out float pitch, out float roll, float robotLatitude, float robotLongitude)
        {
            yaw = 0;
            pitch = robotWalkingDirection;
            roll = MathHelper.PiOver2;

            roll += -MathHelper.Pi * 2 * (robotLatitude / 360);

            yaw = MathHelper.Pi * 2 * (robotLongitude / 360);/**/
        }

        /// <summary>
        /// Gets the tanks position on earth.
        /// </summary>
        /// <param name="latitude">The latitude.</param>
        /// <param name="longitude">The longitude.</param>
        /// <param name="earthRadius">The earth radius.</param>
        /// <returns></returns>
        private Vector3 EarthPosition(float latitude, float longitude, float earthRadius)
        {
            earthRadius += elevation;

            float lat = latitude * (MathHelper.Pi / 180);
            float lon = longitude * (MathHelper.Pi / 180);
            float x = -earthRadius * (float)Math.Cos(lat) * (float)Math.Cos(lon);
            float y = earthRadius * (float)Math.Sin(lat);
            float z = earthRadius * (float)Math.Cos(lat) * (float)Math.Sin(lon);

            return position = new Vector3(x, y, z);
        }

        /// <summary>
        /// Gets the elevation for the specific latitude and longitude.
        /// </summary>
        /// <param name="latitude">The latitude.</param>
        /// <param name="longitude">The longitude.</param>
        /// <param name="elevation">The previous elevation.</param>
        /// <param name="delta">The delta(this is used to smooth the transition from a low place to a high).</param>
        /// <returns></returns>
        private float LocalElevation(float latitude, float longitude, float elevation,float delta)
        {
            float heightDataX = heightData.GetLength(0) - 1;
            float heightDataY = heightData.GetLength(1) - 1;
            float newElevation = 0 ;
            float xled = (longitude + 180f) / (360 / heightDataX);
            float yled = (latitude + 90f) / (180 / heightDataY);
            yled = heightDataY - yled;

            int xIndex = (int)Math.Floor(xled);
            int yIndex = (int)Math.Floor(yled);

            if (xIndex >= 0 && xIndex < heightDataX && yIndex >= 0 && yIndex < heightDataY)
            {
                 newElevation = -(float)((heightData[xIndex, yIndex].h) / 200f) - heightPositionModifier;

            }
            else
            {
                elevation = 0;
            }
            elevation = elevation + delta * (newElevation - elevation);



            return elevation;
        }

    }
}
